#include <math.h>
#include <stdio.h>
#include "common.h"
#include "databank.h"
#include "physics.h"
#include "background.h"
#include "shot.h"
#include "particle.h"
#include "player.h"

// This is a magical 1/sqrt() function that is stolen from far away
/*float invSqrt(float x)
  {
  float xhalf = 0.5f*x;
  int i = *(int*)&x;
  i = 0x5f3759df - (i>>1);
  x = *(float*)&i;
  return x*(1.5f - xhalf*x*x);
  }*/

/* This code was pretty much a direct copy off
 * of wikipedia
 */
float fastsqrt(float val)  {
    union
    {
        int tmp;
        float val;
    } u;
    u.val = val;
    u.tmp -= 1<<23; /* Remove last bit so 1.0 gives 1.0 */
    /* tmp is now an approximation to logbase2(val) */
    u.tmp >>= 1; /* divide by 2 */
    u.tmp += 1<<29; /* add 64 to exponent: (e+127)/2 =(e/2)+63, */
    /* that represents (e/2)-64 but we want e/2 */
    return u.val;
}



bool PhysicsModule::doPhysics(double delta)
{

    // Retrieve the list of zones from the background
    Background *back = data->getBackground();
    std::vector<Zone *> *zones = back->getZones();
    std::vector<Zone *>::iterator zi;


    std::list<Gravitated *>::iterator i;

    // For all list objects to be killed off
    std::list<std::list<Gravitated *>::iterator> kills;

    for(i = gob.begin(); i != gob.end(); i++) {
        Gravitated *g;
        g = *i;

        // Move it around
        for(zi = zones->begin(); zi != zones->end(); zi++) {
            MassCenter *mc = (*zi)->getCenter();
            double r[2];
            double length2, length3;
            float invLength;

            // Find direction towards MassCenter
            r[0] = (mc->pos[0]-g->position[0]);
            r[1] = (mc->pos[1]-g->position[1]);
            length2 = r[0]*r[0]+r[1]*r[1];
            //invLength = invSqrt(length2);

            // Prevent "hypergravitation"
            invLength = fastsqrt(length2);
            if(g->type == SHOT) {
	            double length_min = 8.0*(delta*double(FPS));
	            if(invLength < length_min) invLength = length_min;
	        }
	        
            length3 = invLength*length2;

            //r[0] *= invLength;
            //r[1] *= invLength;

            g->velocity[0] += cfg->gravitationalConstant*(mc->mass*g->mass)
                / length3*r[0]*delta;
            g->velocity[1] += cfg->gravitationalConstant*(mc->mass*g->mass)
                / length3*r[1]*delta;
        }

        g->position[0] += g->velocity[0]*delta;
        g->position[1] += g->velocity[1]*delta;

        switch(g->type)
        {
            case SHOT:
                // Every single frame
                if(g->t.s->doIdle(g->t.s, delta)) {
                    kills.push_back(i);
                    continue;
                }

                // When a shot collides with the background
                if(!is_inside_bitmap(back->getBackground(),
                            g->position[0], g->position[1], 0)) {
                    // Outside the playfield.. DIE!
                    g->t.s->killMe = 1;
                    kills.push_back(i);
                    continue;
                }
                
                // Test collision with players
                // TODO: NUMBER_OF_PLAYERS!
                for(int z=0; z<data->getPlayerSize(); z++) {
                	if(data->getPlayer(z)->alive() && data->getPlayer(z)->testCollision(g->position)) {
                		// We have a collision, be angry!
                		g->t.s->doPlayerCollision(g->t.s, data->getPlayer(z));
                	}
                }

                if(back->testCollision(g->position[0],g->position[1])) {
                    g->t.s->doBackgroundCollision(back, g->t.s);
                }
				
                if(g->t.s->killMe) {
                    kills.push_back(i);
                }
                break;

            case PARTICLE:
                // Particles are handled a different way
                if(!is_inside_bitmap(back->getBackground(),
                            g->position[0], g->position[1], 0)) {
                    // Outside the playfield.. DIE!
                    g->t.p->killMe = 1;
                    kills.push_back(i);
                    continue;
                }

                g->t.p->collideBackground(back,delta);
                
                // Test collision with players
                // TODO: NUMBER_OF_PLAYERS!
                for(int z=0; z<data->getPlayerSize(); z++) {
                	if(data->getPlayer(z)->alive() && data->getPlayer(z)->testCollision(g->position)) {
                		// We have a collision, be angry!
                		g->t.p->collidePlayer(data->getPlayer(z));
                	}
                }
				
                if(g->t.p->killMe) {
                    kills.push_back(i);
                    continue;
                }
                break;
        }
    }

    // Kill all dead objects
    data->cleanParticles();
    data->cleanShots();

    std::list<std::list<Gravitated *>::iterator>::iterator scythe;
    for(scythe = kills.begin(); scythe != kills.end(); scythe++) {
        delete(**scythe);
        gob.erase(*scythe);
    }
    kills.clear();

    return (gob.size()==0);
}

void PhysicsModule::clear()
{
	for(std::list<Gravitated *>::iterator i = gob.begin(); i != gob.end(); i++) {
		delete *i;
	}
	
	gob.clear();
}

